﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO.Ports;

namespace ArduinoComController
{
    public class ArduinoCom
    {
        private SerialPort comPort;

        public enum ComCommand
        {
            UnknownCommand = 0,
            SetMotorATicksSpeed = 1,
            SetMotorBTicksSpeed = 2,

            SetMotorA_Kp = 4,
            SetMotorA_Ki = 5,
            SetMotorA_Kd = 6,

            SetMotor_SampleTime = 7,

            SetMotorB_Kp = 8,
            SetMotorB_Ki = 9,
            SetMotorB_Kd = 10,

            SetServo0_Position = 12,

            Get_Distance_MotorA = 14,
            Get_Distance_MotorB = 15,
            Get_DistanceSensor_Measure = 16,

            GetMotorATicksSpeed = 17,
            GetMotorBTicksSpeed = 18,
            ResetArduino = 23,
        } ;

        public Dictionary<ComCommand, int> ComCommandReturnLength = new Dictionary<ComCommand, int>
            {
                {ComCommand.UnknownCommand, 0},
                {ComCommand.SetMotorATicksSpeed, 0},
                {ComCommand.SetMotorBTicksSpeed, 0},

                {ComCommand.SetMotorA_Kp, 0},
                {ComCommand.SetMotorA_Ki, 0},
                {ComCommand.SetMotorA_Kd, 0},
                {ComCommand.SetMotorB_Kp, 0},
                {ComCommand.SetMotorB_Ki, 0},
                {ComCommand.SetMotorB_Kd, 0},

                {ComCommand.SetServo0_Position, 0},

                {ComCommand.Get_Distance_MotorA, 4},
                {ComCommand.Get_Distance_MotorB, 4},
                {ComCommand.Get_DistanceSensor_Measure, 4},

                {ComCommand.GetMotorATicksSpeed, 4},
                {ComCommand.GetMotorBTicksSpeed, 4},
                {ComCommand.ResetArduino, 0},
            };
        

        public ArduinoCom(string port)
        {
            comPort = new SerialPort(port);
            comPort.BaudRate = 9600;
            if(!comPort.IsOpen)
                comPort.Open();

            comPort.ReadExisting();
        }

        public void ArduinoComClose()
        {
            comPort.Close();
        }

        public Int32 sendCmd(ComCommand cmd, Int16 data)
        {
            var buffer = new byte[4];
            buffer[0] = (byte) cmd;
            buffer[1] = (byte) (data >> 8);
            buffer[2] = (byte) (data & 0xFF);

            comPort.Write(buffer, 0, 3);

            for(var i = 0; i < ComCommandReturnLength[cmd]; i++)
                buffer[i] = (byte) comPort.ReadByte();

            Int32 outputData;
            outputData  = (Int32)(buffer[0] << 24);
            outputData |= (Int32)(buffer[1] << 16);
            outputData |= (Int32)(buffer[2] << 8);
            outputData |= buffer[3];

            return outputData;
        }

        public static double GetAverageNumber(int number1, int number2)
        {
            return (number2 + number1)/2.0;
        }
    }
}
